/*
 * @Description:  
 * @Author: ~光~~
 * @Date: 2024-08-25 10:29:02
 * @LastEditTime: 2024-10-20 19:28:20
 * @LastEditors:  
 */
/*
 * OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
 *
 * Copyright (C) 2022 i2Nav Group, Wuhan University
 *
 *     Author : Hailiang Tang
 *    Contact : thl@whu.edu.cn
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#include "src/common/earth.h"
#include "src/common/types.h"

#include "src/fileio/filesaver.h"
#include "src/fileio/gnssfileloader.h"
#include "src/fileio/imufileloader.h"
#include "src/factors/gnss_factor.h"
#include "src/factors/marginalization_factor.h"
#include "src/factors/pose_manifold.h"
#include "src/preintegration/imu_error_factor.h"
#include "src/preintegration/preintegration.h"
#include "src/preintegration/preintegration_factor.h"

#include <absl/strings/str_format.h>
#include <absl/time/clock.h>
#include <deque>
#include <iomanip>
#include <yaml-cpp/yaml.h>


/*
//main函数分为四部分，几乎主程序都可以如此划分

1.数据参数预处理
主要功能：读取参数文件，在OB_GINS中读取了，滑动窗口次数、处理数据时间段、优化迭代次数、初始速度、位置、IMU相关参数等。

2.程序初始化
主要作用：通过上一步读取的数据进行系统初始化。这个初始化详细讲一下
（1）设置站心坐标系（北东地）原点，更新当地重力参数。
（2）设置预积分选项：是否添加里程计，是否考虑地球自转
（3）初始状态（当前状态）设置：北东地位置、姿态、速度、加速度计陀螺仪偏置、安装角。

3.循环处理数据
（1）imu预积分
（2）利用GNSS数据进行优化

4.结果输出

*/

//INTEGRATION_LENGTH：通常用于控制IMU数据预积分的时间间隔。
#define INTEGRATION_LENGTH 1.0

//MINIMUM_INTERVAL：用于确保两个采样数据之间的时间间隔不小于指定值，以避免处理过于密集的数据。
#define MINIMUM_INTERVAL 0.001

//判断是否需要进行数据内插
int isNeedInterpolation(const IMU &imu0, const IMU &imu1, double mid);

//IMU数据内插
void imuInterpolation(const IMU &imu01, IMU &imu00, IMU &imu11, double mid);

//保存结果
void writeNavResult(double time, const Vector3d &origin, const IntegrationState &state, FileSaver &navfile,
                    FileSaver &errfile);

int main(int argc, char *argv[]) {

    if (argc != 2) {
        std::cout << "usage: ob_gins ob_gins.yaml" << std::endl;
        return -1;
    }

    std::cout << "\nOB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System\n\n";
    //absl::Now() 是 Abseil 库中用于获取当前时间的函数。
    //它返回一个表示当前时间的对象（例如，时间戳），通常以 UTC 时间格式表示
    auto ts = absl::Now();

    // 读取配置
    // load configuration
    /*这部分代码尝试加载 YAML 配置文件，并将其内容存储在 config 中。
    如果读取失败，程序将输出错误信息并退出。*/
    YAML::Node config;
    std::vector<double> vec;
    try {
        config = YAML::LoadFile(argv[1]);
    } catch (YAML::Exception &exception) {
        std::cout << "Failed to read configuration file" << std::endl;
        return -1;
    }

    // 时间信息  从yaml文件中读取相关的信息
    // processing time
    int windows   = config["windows"].as<int>();
    int starttime = config["starttime"].as<int>();
    int endtime   = config["endtime"].as<int>(迭代次数);
    // 尝试获取 "endtime"，如果不存在则使用 迭代次数 的值

    // 迭代次数
    // number of iterations
    int num_iterations = config["num_iterations"].as<int>();

    // 是否进行GNSS粗差检验
    // Do GNSS outlier culling
    bool is_outlier_culling = config["is_outlier_culling"].as<bool>();

    // 初始化
    // initialization
    vec = config["initvel"].as<std::vector<double>>();//北东地 速度
    Vector3d initvel(vec.data());

    vec = config["initatt"].as<std::vector<double>>();
    //俯仰横滚航向
    Vector3d initatt(vec.data());
    initatt *= D2R;

    vec = config["initgb"].as<std::vector<double>>();
    //  初始陀螺零偏 
    Vector3d initbg(vec.data());
    initbg *= D2R / 3600.0;

    vec = config["initab"].as<std::vector<double>>(); 
    //初始加速度计零偏 
    Vector3d initba(vec.data());
    initba *= 1.0e-5;

    // 数据文件
    // data file
    std::string gnsspath   = config["gnssfile"].as<std::string>();
    std::string imupath    = config["imufile"].as<std::string>();
    std::string outputpath = config["outputpath"].as<std::string>();
    int imudatalen         = config["imudatalen"].as<int>();
    int imudatarate        = config["imudatarate"].as<int>();

    // 是否考虑地球自转
    // consider the Earth's rotation
    bool isearth = config["isearth"].as<bool>();

    GnssFileLoader gnssfile(gnsspath);
    ImuFileLoader imufile(imupath, imudatalen, imudatarate);
    FileSaver navfile(outputpath + "/OB_GINS_TXT.nav", 11, FileSaver::TEXT);
    FileSaver errfile(outputpath + "/OB_GINS_IMU_ERR.bin", 7, FileSaver::BINARY);
    if (!imufile.isOpen() || !navfile.isOpen() || !navfile.isOpen() || !errfile.isOpen()) {
        std::cout << "Failed to open data file" << std::endl;
        return -1;
    }

    // 安装参数
    // installation parameters 各个坐标系下的参数
    vec = config["antlever"].as<std::vector<double>>();
    Vector3d antlever(vec.data());
    vec = config["odolever"].as<std::vector<double>>();
    Vector3d odolever(vec.data());
    vec = config["bodyangle"].as<std::vector<double>>();
    Vector3d bodyangle(vec.data());
    bodyangle *= D2R;

    // IMU噪声参数
    // IMU noise parameters
    auto parameters          = std::make_shared<IntegrationParameters>();
    parameters->gyr_arw      = config["imumodel"]["arw"].as<double>() * D2R / 60.0;
    parameters->gyr_bias_std = config["imumodel"]["gbstd"].as<double>() * D2R / 3600.0;
    parameters->acc_vrw      = config["imumodel"]["vrw"].as<double>() / 60.0;
    parameters->acc_bias_std = config["imumodel"]["abstd"].as<double>() * 1.0e-5;
    parameters->corr_time    = config["imumodel"]["corrtime"].as<double>() * 3600;

    bool isuseodo       = config["odometer"]["isuseodo"].as<bool>();
    vec                 = config["odometer"]["std"].as<std::vector<double>>();
    parameters->odo_std = Vector3d(vec.data());
    parameters->odo_srw = config["odometer"]["srw"].as<double>() * 1e-6;
    parameters->lodo    = odolever;
    parameters->abv     = bodyangle;

    // GNSS仿真中断配置
    // GNSS outage parameters
    bool isuseoutage = config["isuseoutage"].as<bool>();
    int outagetime   = config["outagetime"].as<int>();
    int outagelen    = config["outagelen"].as<int>();
    int outageperiod = config["outageperiod"].as<int>();

    auto gnssthreshold = config["gnssthreshold"].as<double>();

    // 数据文件调整
    /* 
        这段代码实现了从IMU（惯性测量单元）和GNSS（全球导航卫星系统）文件中读取数据，
        并找到数据时间戳大于或等于 starttime 的数据行。
    */
    // data alignment 数据对准
    
    IMU imu_cur, imu_pre;
    do {
        imu_pre = imu_cur;
        imu_cur = imufile.next();
    } while (imu_cur.time < starttime);

    GNSS gnss;
    do {
        gnss = gnssfile.next();
    } while (gnss.time < starttime);

    // 初始位置, 求相�?
    /*
    处理GNSS位置数据，将其转换成局部坐标系并计算重力
    */


    //这行代码将 gnss.blh（BLH代表地理坐标，通常是经度、纬度、高度）赋值给一个变量 station_origin，将其作为基准点的初始位置。
    //这个操作的目的是保存最初的位置，以便在后续计算中将其他坐标点转换为相对于这个位置的局部坐标。
    Vector3d station_origin = gnss.blh;
    //这行代码使用 Earth::gravity() 函数计算在 gnss.blh 所在位置的重力加速度，
    //并将结果存储在 parameters->gravity 中
    parameters->gravity     = Earth::gravity(gnss.blh);
    
    /*
    这一行代码将 gnss.blh 从地理坐标系（全球坐标系）转换为局部坐标系。
    Earth::global2local() 函数将使用 station_origin 作为局部坐标系的原点，
    将当前位置 gnss.blh 转换成相对于 station_origin 的局部坐标。
    */
    gnss.blh                = Earth::global2local(station_origin, gnss.blh);

    // 站心坐标系
    parameters->station = station_origin;

    /*
    这段代码是在初始化和设置导航系统的数据结构，包括状态信息、GNSS 数据、预积分信息、以及时间戳。
    这些数据结构对于状态估计、滤波、预积分计算等过程非常重要。使用 std::vector 和 std::deque 的
    组合方式，既保证了对历史数据的存储，又可以灵活地进行数据的插入和删除
    */
    //windows 是一个整数变量，用于定义 statelist 向量的大小。它指定了需要存储多少个 IntegrationState 对象。
    //向量的大小被设置为 windows + 1，这意味着 statelist 将包含 windows + 1 个 IntegrationState 元素。
    std::vector<IntegrationState> statelist(windows + 1);
    std::vector<IntegrationStateData> statedatalist(windows + 1);
    //表示预积分数据的基类，用于管理惯性导航系统（INS）的积分数据。
    //预积分是一种用于减少计算复杂度的方法，在惯性导航中常用。
    std::deque<std::shared_ptr<PreintegrationBase>> preintegrationlist;
    std::deque<GNSS> gnsslist;
    std::deque<double> timelist;

    Preintegration::PreintegrationOptions preintegration_options = Preintegration::getOptions(isuseodo, isearth);

    // 初始状态
    /*
    这段代码的主要功能是初始化当前状态 state_curr，基于 GNSS 数据、姿态、速度、天线位置偏移量等信息，
    设置导航系统的初始位置、姿态、速度以及传感器偏差等状态参数。
    */
    IntegrationState state_curr = {
        .time = round(gnss.time),
        .p    = gnss.blh - Rotation::euler2quaternion(initatt) * antlever,
        .q    = Rotation::euler2quaternion(initatt),
        .v    = initvel,
        .bg   = initbg,
        .ba   = initba,
        .sodo = 0.0,
        .abv  = {bodyangle[1], bodyangle[2]},
    };
    std::cout << "Initilization at " << gnss.time << " s " << std::endl;

    /*
        这段代码的主要目的是初始化和存储导航系统中的状态、GNSS 数据、时间信息等关键数据，
        以便后续在 GNSS/INS（全球导航卫星系统/惯性导航系统）集成过程中使用。
        这些数据将用于后续的状态估计、预积分（Preintegration）计算以及系统优化。
    */
    statelist[0]     = state_curr;
    statedatalist[0] = Preintegration::stateToData(state_curr, preintegration_options);
    gnsslist.push_back(gnss);
    
    double sow = round(gnss.time);
    timelist.push_back(sow);

    // 初始预积分
    // Initial preintegration
    /*
    GNSS/INS（全球导航卫星系统/惯性导航系统）集成系统中的预积分（Preintegration）计算和
    GNSS 数据处理。它的主要目标是通过预积分来处理惯性测量单元（IMU）数据，
    并结合 GNSS 数据更新系统状态，同时准备下一次的状态估计和边缘化。
    */
   
   /*
    1.预积分对象的创建：创建用于处理 IMU 数据的预积分对象，并将其存储在列表中，以便后续状态估计时使用。
    2.更新 GNSS 数据：读取新的 GNSS 数据，并在局部坐标系中对其进行转换，以用于导航状态的校正。
    3.边缘化信息的准备：初始化边缘化相关的数据结构，以提高后续状态估计的计算效率。
    4.推进时间步长：增加时间变量，为下一阶段的积分和状态估计做准备
    
    通过 emplace_back 方法，将新创建的预积分对象添加到 preintegrationlist 列表中，
    这个列表用于存储所有的预积分对象.
    
    这里调用了 Preintegration::createPreintegration 函数，创建了一个新的预积分对象，
    用于处理从当前状态到下一状态的 IMU 数据。
    传入的参数包括：
    parameters：包含 IMU 噪声、重力、比例因子等信息的结构体。
    imu_pre：上一时刻的 IMU 数据。
    state_curr：当前的导航状态。
    preintegration_options：预积分选项，用于控制预积分计算的配置。

    预积分是将 IMU 数据从一个状态预积分到下一个状态的过程。
    这一步将 IMU 数据封装成一个预积分对象，方便后续进行状态估计和校正。
   */
    preintegrationlist.emplace_back(
        Preintegration::createPreintegration(parameters, imu_pre, state_curr, preintegration_options));

    /*
    使用 gnssfile.next() 读取下一个 GNSS 数据点。
    更新 parameters->gravity 为根据新的 GNSS 坐标计算出的当地重力值。
    使用 Earth::global2local 函数，将 GNSS 坐标（全球坐标）转换为局部坐标系中的坐标（相对于起始位置），
    以便在导航状态估计中使用。
    通过读取新的 GNSS 数据并进行重力值计算和坐标转换，使得系统能够在局部坐标系中更准确地处理 GNSS 和惯性导航数据。
    */
    // 读取下一个整秒GNSS
    gnss                = gnssfile.next();
    parameters->gravity = Earth::gravity(gnss.blh);
    gnss.blh            = Earth::global2local(station_origin, gnss.blh);


    
    /*
    last_marginalization_info 是一个共享指针，用于保存上一轮边缘化过程中产生的信息。
    last_marginalization_parameter_blocks 是一个向量，用于存储边缘化过程中涉及的参数块指针。

    边缘化是一种在状态估计中减少状态维度的方法。通过保存上一轮的边缘化信息，
    可以提高系统的计算效率并减少状态向量的复杂度。
    */
    std::shared_ptr<MarginalizationInfo> last_marginalization_info;
    std::vector<double *> last_marginalization_parameter_blocks;

    /*
    这里将 GNSS 时间（以秒为单位）增加一个固定值 INTEGRATION_LENGTH。
    这个操作意味着准备进入下一个时间段的积分计算。
    作用：通过增加积分长度来推进系统的时间步长，使得导航状态估计能够进入下一阶段。
    这一步是预积分和状态更新过程中的关键步骤。
    */
    sow += INTEGRATION_LENGTH;

    while (true) {
        if ((imu_cur.time > endtime) || imufile.isEof()) {
            break;
        }

        // 加入IMU数据并进行预积分
        // Add new imu data to preintegration
        /*
            调用 preintegrationlist.back() 会返回最后一个 PreintegrationBase 
            对象的智能指针，你可以使用 -> 来调用它的成员函数。

            这行代码将当前的IMU数据（imu_cur）添加到一个预积分列表中。
            预积分是IMU算法中常用的一种技术，用来在两个时间点之间累积变化量（例如速度或位置）
        */
        preintegrationlist.back()->addNewImu(imu_cur);

        imu_pre = imu_cur;
        imu_cur = imufile.next();

        if (imu_cur.time > sow) {
            // 当前IMU数据时间等于GNSS数据时间, 读取新的GNSS
            /*
            检查IMU数据的时间是否大于当前时间标记（sow）。如果是，说明需要处理GNSS数据了。
            if (fabs(gnss.time - sow) < MINIMUM_INTERVAL)：
            检查GNSS数据的时间与当前时间标记（sow）之间的差是否小于一个最小时间间隔（MINIMUM_INTERVAL）
            
            如果是，则认为GNSS数据有效，将其添加到gnsslist列表中

            这大段if 的作用：
            这段代码的总体作用是对IMU和GNSS数据进行同步、预处理和中断处理，
            以确保它们在随后的计算中可以被正确使用。
            */
    
            if (fabs(gnss.time - sow) < MINIMUM_INTERVAL) {
                gnsslist.push_back(gnss);

                /*
                    gnss = gnssfile.next();：从GNSS文件中读取下一个GNSS数据。
                   这个循环会一直读取新的GNSS数据，直到它满足精度要求（即GNSS标准差小于指定阈值gnssthreshold）
                */
                gnss = gnssfile.next();

                //相当于抗差了 忽略误差大的 GNSS 数据，系统能够在误差较小时进行有效校正，保持导航过程的稳定性。
                while ((gnss.std[0] > gnssthreshold) || (gnss.std[1] > gnssthreshold) ||
                       (gnss.std[2] > gnssthreshold)) {
                    gnss = gnssfile.next();
                }

                // 中断配置
                // do GNSS outage
                /*
                if (isuseoutage)：检查是否启用了GNSS中断。
                如果当前的GNSS时间等于中断时间点（outagetime），打印中断信息，
                并在接下来的outagelen秒中跳过GNSS数据。

                lround 是 C++ 标准库中的一个函数，用于将一个浮点数四舍五入为最接近的长整数（long int）类型。
                它的全称是 "long round"，定义在 <cmath> 头文件中。这个函数会根据浮点数的小数部分进行四舍五入，
                并返回一个 long 类型的整数值。
                */
                if (isuseoutage) {
                    if (lround(gnss.time) == outagetime) {
                        std::cout << "GNSS outage at " << outagetime << " s" << std::endl;
                        for (int k = 0; k < outagelen; k++) {
                            gnss = gnssfile.next();
                        }
                        outagetime += outageperiod;//更新下一个中断时间点
                    }
                }

                //坐标转换 更新新的重力值 和 经纬高值
                parameters->gravity = Earth::gravity(gnss.blh);
                gnss.blh            = Earth::global2local(station_origin, gnss.blh);

                //如果GNSS文件已经读取到结尾，则将GNSS时间设为0，表示没有更多的数据
                if (gnssfile.isEof()) {
                    gnss.time = 0;
                }
            }

            // IMU内插处理
            // IMU interpolation
            int isneed = isNeedInterpolation(imu_pre, imu_cur, sow);
            if (isneed == -1) {
            } else if (isneed == 1) {//表示当前IMU数据已经满足条件，直接将 imu_cur 添加到预积分列表中。
                preintegrationlist.back()->addNewImu(imu_cur);

                imu_pre = imu_cur;
                imu_cur = imufile.next();
            } else if (isneed == 2) {
                //进行插值
                /*调用了一个名为 imuInterpolation 的函数，
                用来在之前的IMU数据（imu_pre）和当前IMU数据（imu_cur）之间进行插值操作，
                然后将插值结果添加到预积分列表中*/
                imuInterpolation(imu_cur, imu_pre, imu_cur, sow);
                preintegrationlist.back()->addNewImu(imu_pre);
            }

            // 下一个积分节点
            /*最后，将当前的时间标记 sow 添加到时间列表 timelist 中，
            并将 sow 增加一个积分时间长度 INTEGRATION_LENGTH，为下一个时间节点做准备*/
            // next time node
            timelist.push_back(sow);
            sow += INTEGRATION_LENGTH;
            /*
            state_curr：从预积分列表（preintegrationlist）中获取当前的状态，并将其存储到滑动窗口的状态
            列表（statelist）和状态数据列表（statedatalist）中。
            */    
            // 当前整秒状态加入到滑窗
            state_curr                               = preintegrationlist.back()->currentState();
            statelist[preintegrationlist.size()]     = state_curr;
            statedatalist[preintegrationlist.size()] = Preintegration::stateToData(state_curr, preintegration_options);

            // 构建优化问题
            // construct optimization problem
            {

                /*
                这里创建了一个Ceres求解器的问题对象和求解选项：
                enable_fast_removal = true：启用快速移除功能，用于在滑动窗口中有效地移除旧的数据。
                使用 Levenberg-Marquardt 算法作为信赖域策略，并采用 稀疏正态方程求解器（SPARSE_NORMAL_CHOLESKY） 来处理优化问题。
                将线程数设置为 4，以提高求解速度。
                */
                ceres::Problem::Options problem_options;
                //enable_fast_removal：设置为 true 时，表示启用快速移除功能。
                //它允许在优化过程中高效地移除旧的参数块，提升计算效率
                problem_options.enable_fast_removal = true;
                
                ceres::Problem problem(problem_options);
                ceres::Solver solver;
                ceres::Solver::Summary summary;
                ceres::Solver::Options options;

                //ceres::LEVENBERG_MARQUARDT：是一种常用的信赖域策略，
                //结合了梯度下降和高斯牛顿方法的特点，能够在处理非线性最小二乘问题时表现出色。
                options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;

                //指定线性求解器的类型，用于解决优化问题中的线性子问题。
                //SPARSE_NORMAL_CHOLESKY适用于稀疏矩阵的求解器，它基于Cholesky分解，效率较高且内存占用较低，适合大型问题和具有稀疏结构的问题。
                options.linear_solver_type  = ceres::SPARSE_NORMAL_CHOLESKY;

                //设置用于求解器的线程数。
                //意味着求解器将在多线程环境下运行，使用最多 4 个线程。
                options.num_threads                = 4;

                // 添加参数块
                // add parameter blocks
                for (size_t k = 0; k <= preintegrationlist.size(); k++) {
                    // 位姿

                    /*
                    manifold：这是一个Ceres库中的概念，用于定义参数在优化过程中如何更新。

                    在非线性优化中，某些变量（如姿态）不是简单的欧几里得空间变量，而是定义在某种流形（manifold）上。
                    比如位姿（位置和旋转）通常定义在李群（Lie Group）上，需要特殊的处理。

                    PoseManifold 是一个自定义的流形类，专门用来处理位姿参数的优化。
                    它确保旋转部分在优化过程中遵循适当的更新规则（例如四元数的归一化）。 
                    */
                    ceres::Manifold *manifold = new PoseManifold();


                    /*
                    statedatalist[k].pose：代表需要优化的位姿（位置和姿态）参数。这些参数的更新受 PoseManifold 约束，以确保旋转部分的正确性。
                    statedatalist[k].mix：代表其他与IMU相关的混合参数，如传感器的偏差。这些参数的优化帮助修正传感器误差，提高估计精度
                    
                    AddParameterBlock：这是Ceres求解器用来添加需要优化的参数块的方法。
                    statedatalist[k].pose：这个参数表示位姿（位置和姿态）的数据。它通常包含三维位置（x, y, z）和四元数或欧拉角形式的姿态表示。
                    Preintegration::numPoseParameter()：这边是7 返回位姿参数的数量，即表示位姿所需的参数维数（例如位置的三个参数和姿态的四个参数）。
                    manifold：提供了位姿参数的更新规则，以确保在优化过程中姿态部分保持合法（例如四元数的单位长度）。
                    */
                    problem.AddParameterBlock(statedatalist[k].pose, Preintegration::numPoseParameter(), manifold);

                    /*
                    statedatalist[k].mix：这个参数表示与IMU状态相关的其他参数，比如传感器的偏差、误差项或者其他需要优化的混合参数。
                    这些混合参数（mix）可能包括：
                    加速度计偏差（Bias）：表示传感器测量时的系统性偏差。

                    陀螺仪偏差（Gyroscope Bias）：类似地，用于描述陀螺仪测量中的系统性误差。

                    Preintegration::numMixParameter(preintegration_options)：返回混合参数的数量，
                    具体的维度取决于预积分选项（preintegration_options）的配置。
                    例如，可能包含6个参数来表示三维的加速度计偏差和陀螺仪偏差。
                    */
                    problem.AddParameterBlock(statedatalist[k].mix,
                                              Preintegration::numMixParameter(preintegration_options));
                }

        //下面开始各个因子的构建---残差的计算
                // GNSS残差
                // GNSS factors
                /*
                这段代码的主要功能是将 GNSS 数据转化为优化问题中的因子（即残差块），并将这些因子添加到 Ceres 优化问题中。
                它通过使用 GNSS 和惯性数据的时间匹配来构建优化问题，以求解导航系统中的位姿和速度
               
                GnssFactor：表示 GNSS 观测的因子，它将 GNSS 数据（如位置或速度）与状态变量关联起来，生成残差项。
                problem.AddResidualBlock：将 GNSS 残差块添加到优化问题中。使用了 loss_function 来处理异常值。
                statedatalist[i].pose：GNSS 因子会影响当前时刻的位姿状态。
                
                每当找到一个时间匹配（fabs(gnss.time - timelist[i]) < MINIMUM_INTERVAL）时，就将对应的 GNSS 残差块添加到问题中，并保存其 ID。
                */
                int index = 0;

                ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);//loss_function：这里使用了 Huber Loss 损失函数
                std::vector<std::pair<double, ceres::ResidualBlockId>> gnss_residualblock_id;
                for (const auto &gnss : gnsslist) {
                    /*
                    GnssFactor 是 GNSS 数据的因子类，它将 GNSS 观测数据（如位置、速度）与状态变量关联起来，生成误差项。
                    antlever 是天线与惯性测量单元（IMU）之间的偏移量，用于对 GNSS 数据进行校正
                    */
                    auto factor = new GnssFactor(gnss, antlever);

                    /*
                    遍历状态变量列表 timelist，查找与当前 GNSS 数据时间戳匹配的状态时间。
                    如果找到时间匹配的状态变量（fabs(gnss.time - timelist[i]) < MINIMUM_INTERVAL），
                    则认为当前时刻的 GNSS 数据与状态变量时间相符。
                    */
                    for (size_t i = index; i <= preintegrationlist.size(); ++i) {
                        if (fabs(gnss.time - timelist[i]) < MINIMUM_INTERVAL) {
                            //使用 problem.AddResidualBlock() 函数将 GNSS 残差块添加到优化问题中，并获取该残差块的 ResidualBlockId
                            /*
                            将 GNSS 残差块添加到优化问题中。problem 是 Ceres 优化问题对象，factor 是残差因子，
                            loss_function 是损失函数，statedatalist[i].pose 是状态变量。
                            添加残差块后，Ceres 会根据 GNSS 数据生成残差，并在优化过程中尝试最小化这些残差
                            */
                            auto id = problem.AddResidualBlock(factor, loss_function, statedatalist[i].pose);
                            //将 GNSS 数据点的时间戳和对应的 ResidualBlockId 组合成一个 std::pair，然后存入 gnss_residualblock_id 向量中。
                            gnss_residualblock_id.push_back(std::make_pair(gnss.time, id));
                            index++;
                            break;
                        }
                    }
                }

                // 预积分残差
                /*
                构建IMU数据的误差模型：预积分因子将两个相邻时间节点的状态变量连接起来，以确保IMU测量数据与状态估计一致。
                优化问题中残差的添加：通过 problem.AddResidualBlock() 将预积分因子与相关的状态变量关联，
                确保在优化过程中能够对相邻时间节点之间的位姿和偏置进行调整，以最小化IMU测量的误差。

                PreintegrationFactor：这是一个用来处理IMU数据的因子，它使用预积分技术将连续的IMU测量转换为一个残差（误差）。
                这种方法减少了状态变量之间的依赖性，并提高了计算效率。

                statedatalist[k].pose：表示时间节点 k 的位姿（位置和姿态）参数。
                statedatalist[k].mix：表示时间节点 k 的混合参数（如IMU偏置）。
                statedatalist[k + 1].pose：表示时间节点 k+1 的位姿参数。
                statedatalist[k + 1].mix：表示时间节点 k+1 的混合参数

                
                */
                // preintegration factors
                for (size_t k = 0; k < preintegrationlist.size(); k++) {
                    //PreintegrationFactor这是一个因子类，表示预积分数据在优化问题中的残差块（Residual Block）。
                    //该因子通过将预积分的测量值与当前和下一个状态之间的差异关联起来，从而对优化问题施加约束。
                    auto factor = new PreintegrationFactor(preintegrationlist[k]);
                    /*
                    AddResidualBlock 是一个函数，用于将残差块添加到 Ceres 优化问题中。
                    在这里，它会添加一个 PreintegrationFactor，将其与多个状态变量关联，以最小化因子生成的残差。

                    statedatalist 是一个状态变量列表，其中每个元素表示在不同时间点上的导航状态（如位置、速度、姿态等）。
                    这些状态在优化过程中会被调整，以最小化由因子（如 PreintegrationFactor）生成的残差。
                    */
                    problem.AddResidualBlock(factor, nullptr, statedatalist[k].pose, statedatalist[k].mix,
                                             statedatalist[k + 1].pose, statedatalist[k + 1].mix);
                }
                {
                    // IMU误差因子
                    // add IMU bias-constraint factors
                    /*目的：添加IMU误差控制因子到优化问题中，用于对IMU偏置进行约束。
                    ImuErrorFactor：这个因子主要用于描述IMU传感器的偏置误差。在IMU数据中，偏置误差会累积，
                    导致估计值逐渐偏离真实值，因此需要通过优化进行校正。
                    statedatalist[preintegrationlist.size()].mix：表示最后一个时间节点的混合参数，用于存储IMU的偏置估计值。
                    */
                    auto factor = new ImuErrorFactor(*preintegrationlist.rbegin());
                    problem.AddResidualBlock(factor, nullptr, statedatalist[preintegrationlist.size()].mix);
                }

                // 边缘化残差
                // prior factor
                /*
                目的：添加边缘化因子到优化问题中，用于处理先验信息，以进一步约束当前状态。
                MarginalizationFactor：边缘化因子是一种常用的方法，用于在优化过程中移除历史数据的影响，同时保留其对当前状态估计的约束作用。这在滑动窗口优化或连续估计问题中尤为重要。
                
                last_marginalization_info：包含上一次优化过程中边缘化得到的信息。
                last_marginalization_info->isVlid这是一个方法，用来检查边缘化信息是否有效。
                如果信息有效，说明上一次边缘化操作成功完成，并且可以用这些信息来构建边缘化因子。
                last_marginalization_parameter_blocks：表示与先验信息相关联的参数块。
             
                保持先验约束：通过添加边缘化因子，可以将之前的优化信息融入到当前的状态估计中，确保当前的解能够充分利用历史数据。
                提高计算效率：边缘化可以减少当前优化问题的维度
                */

                if (last_marginalization_info && last_marginalization_info->isValid()) {
                    auto factor = new MarginalizationFactor(last_marginalization_info);
                    problem.AddResidualBlock(factor, nullptr, last_marginalization_parameter_blocks);
                }

                /*
                这段代码的核心是在进行最小二乘解求解之后，利用卡方（chi-square）检验对GNSS数据进行异常值剔除。
                下面详细解释这个过程，包括涉及的变量、函数，以及每一步的逻辑。
                */

                // solve the Least-Squares problem
                options.max_num_iterations = num_iterations / 4;//最大迭代次数
                solver.Solve(options, &problem, &summary);//执行最小二乘求解过程，将优化结果存储在 summary 中。

                // TODO: Just a example, you need remodify.
                // Do GNSS outlier culling using chi-square test
                /*
                GNSS异常数据剔除
                
                卡方检验：使用卡方分布进行异常值检测，其中 chi2_threshold 是异常值的阈值，3个自由度下显著水平为0.05时的卡方值为 7.815。
                gnss_outlier：一个 std::unordered_set 集合，用来存储被判定为异常的GNSS数据时间点。

                */
                if (is_outlier_culling && !gnss_residualblock_id.empty()) {
                    // 3 degrees of freedom, 0.05
                    double chi2_threshold = 7.815;
                    
                    // Find GNSS outliers in the window
                    std::unordered_set<double> gnss_outlier;//用来存储被判定为异常的GNSS数据时间点
                    for (size_t k = 0; k < gnsslist.size(); k++) {
                        auto time = gnss_residualblock_id[k].first;
                        auto id   = gnss_residualblock_id[k].second;

                        double cost;
                        double chi2;

                        /*使用 problem.EvaluateResidualBlock() 函数计算残差块的代价 cost，
                        然后通过 chi2 = cost * 2 将其转换为卡方值。
                        problem.EvaluateResidualBlock()：用于计算指定残差块的代价（或误差）。
                            参数包括：
                            id：残差块的ID。
                            false：表示不计算雅可比矩阵。
                            &cost：用于存储计算得到的代价值。
                        */
                        problem.EvaluateResidualBlock(id, false, &cost, nullptr, nullptr);
                        chi2 = cost * 2;

                        if (chi2 > chi2_threshold) {
                            gnss_outlier.insert(time);

                            // Reweigthed GNSS
                            double scale = sqrt(chi2 / chi2_threshold);
                            /* 重加权（Reweighted GNSS）：计算一个缩放因子 scale，
                            并将 gnsslist[k].std 乘以该缩放因子进行调整，以降低异常数据对最终解的影响。*/
                            gnsslist[k].std *= scale;
                        }
                    }
                    // // Log outliers
                    // if (!gnss_outlier.empty()) {
                    //     std::string log = absl::StrFormat("Reweight GNSS outlier at %g:", sow - 1);
                    //     for (const auto& time:gnss_outlier) {
                    //         absl::StrAppendFormat(&log, " %g", time);
                    //     }
                    //     std::cout << log << std::endl;
                    // }

                    // Remove all old GNSS factors
                    //调用Ceres的这个函数来从优化问题中移除指定ID的残差块。
                    for (const auto &block : gnss_residualblock_id) {
                        problem.RemoveResidualBlock(block.second);
                    }

                    // Add GNSS factors without loss function
                    /*
                    这部分代码重新将GNSS因子添加到优化问题 problem 中，不再使用损失函数来处理异常值：
                    gnsslist：包含所有GNSS数据的列表，每个元素表示一个GNSS观测值。
                    GnssFactor(gnss, antlever)：创建一个新的GNSS因子对象，将观测值 gnss 和
                    天线杠杆臂 antlever 作为输入。GNSS因子用于将GNSS数据与状态变量关联起来，以生成残差项
                    */
                    index = 0;
                    for (auto &gnss : gnsslist) {
                        auto factor = new GnssFactor(gnss, antlever);
                        for (size_t i = index; i <= preintegrationlist.size(); ++i) {
                            if (fabs(gnss.time - timelist[i]) < MINIMUM_INTERVAL) {
                                problem.AddResidualBlock(factor, nullptr, statedatalist[i].pose);
                                index++;
                                break;
                            }
                        }
                    }
                }

                options.max_num_iterations = num_iterations * 3 / 4;
                solver.Solve(options, &problem, &summary);

                // 输出进度
                // output the percentage
                int percent            = ((int) sow - starttime) * 100 / (endtime - starttime);
                static int lastpercent = 0;
                if (abs(percent - lastpercent) >= 1) {
                    lastpercent = percent;
                    std::cout << "Percentage: " << std::setw(3) << percent << "%\r";
                    flush(std::cout);
                }
            }
            //这段代码涉及到边缘化、滑动窗口（sliding window）处理、预积分因子的创建和结果的写入

            if (preintegrationlist.size() == static_cast<size_t>(windows)) {
                {
                    // 边缘
                    // marginalization
                    /*
                    marginalization_info 是一个用于管理边缘化过程的对象。它收集所有需要边缘化的信息，包括残差块和状态变量。
                    */
                    std::shared_ptr<MarginalizationInfo> marginalization_info = std::make_shared<MarginalizationInfo>();
                    
                    /*
                    这段代码将上一次边缘化的信息添加到当前的 marginalization_info 中。如果有有效的上次边缘化信息，
                    它会检查是否需要继续边缘化旧的状态变量。marginilized_index 存储需要边缘化的状态索引。

                    在滑动窗口优化中，每次更新优化窗口时，通常会将旧的状态变量从优化问题中移除，
                    但会保留其对系统整体的影响。这就需要对这些状态进行“边缘化”（marginalization），并将影响保留下来。

                    last_marginalization_parameter_blocks：这是一个存储了上次边缘化过程中涉及到的状态变量的列表。
                    遍历 last_marginalization_parameter_blocks，检查是否包含当前窗口中最旧的状态变量
                    （即 statedatalist[0].pose 或 statedatalist[0].mix）。
                    如果找到这些变量，就将它们的索引存储到 marginilized_index 中。这些索引表示哪些参数需要在这次边缘化过程中进行处理。

                    功能：在进行新的边缘化操作之前，首先检查是否存在有效的上一次边缘化信息。
                    意义：只有在存在且有效的情况下，才会将上一次边缘化的因子信息转移到新的边缘化中，
                    这样能够保证历史状态的信息在当前优化过程中得到利用

                    */
                    if (last_marginalization_info && last_marginalization_info->isValid()) {
                      //  找出要边缘化的索引
                        std::vector<int> marginilized_index;
                        //边缘化的目标是将最旧的状态从当前优化问题中剔除，因此需要将这些参数块的索引记录下来。
                        for (size_t k = 0; k < last_marginalization_parameter_blocks.size(); k++) {
                            if (last_marginalization_parameter_blocks[k] == statedatalist[0].pose ||
                                last_marginalization_parameter_blocks[k] == statedatalist[0].mix) {
                                marginilized_index.push_back(static_cast<int>(k));
                            }
                        }

                    /*
                    基于上一次的边缘化信息，创建一个新的 MarginalizationFactor 对象。
                    意义：MarginalizationFactor 用于在优化问题中表示边缘化过程的约束条件，
                    使得之前边缘化掉的信息在新的优化问题中能够继续影响当前和未来状态。
                    */
                        auto factor   = std::make_shared<MarginalizationFactor>(last_marginalization_info);
                       
                        /*
                        创建一个 ResidualBlockInfo 对象，包含了因子、参数块和边缘化索引等信息。
                        factor 是之前创建的 MarginalizationFactor 对象。
                        nullptr 表示不使用损失函数。
                        last_marginalization_parameter_blocks 包含了所有涉及的参数块。
                        marginilized_index 是需要在当前边缘化过程中移除的参数块的索引。
                        ResidualBlockInfo 是边缘化信息的核心部分，它封装了残差块的详细信息，并将这些信息与当前的优化问题进行关联。
                        */
                        
    
                        auto residual = std::make_shared<ResidualBlockInfo>(
                            factor, nullptr, last_marginalization_parameter_blocks, marginilized_index);
                        marginalization_info->addResidualBlockInfo(residual);
                    }

                    // IMU残差
                    // preintegration factors
                    {
                        auto factor   = std::make_shared<PreintegrationFactor>(preintegrationlist[0]);
                        auto residual = std::make_shared<ResidualBlockInfo>(
                            factor, nullptr,
                            std::vector<double *>{statedatalist[0].pose, statedatalist[0].mix, statedatalist[1].pose,
                                                  statedatalist[1].mix},
                            std::vector<int>{0, 1});
                        marginalization_info->addResidualBlockInfo(residual);
                    }

                    // GNSS残差
                    // GNSS factors
                    {
                        if (fabs(timelist[0] - gnsslist[0].time) < MINIMUM_INTERVAL) {
                            auto factor   = std::make_shared<GnssFactor>(gnsslist[0], antlever);
                            auto residual = std::make_shared<ResidualBlockInfo>(
                                factor, nullptr, std::vector<double *>{statedatalist[0].pose}, std::vector<int>{});
                            marginalization_info->addResidualBlockInfo(residual);
                        }
                    }

                    // 边缘化处�?
                    // do marginalization
                    marginalization_info->marginalization();

                    // 数据指针调整
                    // get new pointers
                    std::unordered_map<long, double *> address;
                    for (size_t k = 1; k <= preintegrationlist.size(); k++) {
                        address[reinterpret_cast<long>(statedatalist[k].pose)] = statedatalist[k - 1].pose;
                        address[reinterpret_cast<long>(statedatalist[k].mix)]  = statedatalist[k - 1].mix;
                    }
                    last_marginalization_parameter_blocks = marginalization_info->getParamterBlocks(address);
                    last_marginalization_info             = std::move(marginalization_info);
                }

                // 滑窗处理
                // sliding window
                {
                    if (lround(timelist[0]) == lround(gnsslist[0].time)) {
                        gnsslist.pop_front();
                    }
                    timelist.pop_front();
                    preintegrationlist.pop_front();

                    for (int k = 0; k < windows; k++) {
                        statedatalist[k] = statedatalist[k + 1];
                        statelist[k]     = Preintegration::stateFromData(statedatalist[k], preintegration_options);
                    }
                    statelist[windows] = Preintegration::stateFromData(statedatalist[windows], preintegration_options);
                    state_curr         = statelist[windows];
                }
            } else {
                state_curr =
                    Preintegration::stateFromData(statedatalist[preintegrationlist.size()], preintegration_options);
            }

            // write result
            writeNavResult(*timelist.rbegin(), station_origin, state_curr, navfile, errfile);

            // 新建立新的预积分
            // build a new preintegration object
            preintegrationlist.emplace_back(
                Preintegration::createPreintegration(parameters, imu_pre, state_curr, preintegration_options));
        } else {
            auto integration = *preintegrationlist.rbegin();
            writeNavResult(integration->endTime(), station_origin, integration->currentState(), navfile, errfile);
        }
    }

    navfile.close();
    errfile.close();
    imufile.close();
    gnssfile.close();

    auto te = absl::Now();
    std::cout << std::endl << std::endl << "Cost " << absl::ToDoubleSeconds(te - ts) << " s in total" << std::endl;

    return 0;
}




void writeNavResult(double time, const Vector3d &origin, const IntegrationState &state, FileSaver &navfile,
                    FileSaver &errfile) {
    vector<double> result;

    Vector3d pos = Earth::local2global(origin, state.p);
    pos.segment(0, 2) *= R2D;
    Vector3d att = Rotation::quaternion2euler(state.q) * R2D;
    Vector3d vel = state.v;
    Vector3d bg  = state.bg * R2D * 3600;
    Vector3d ba  = state.ba * 1e5;

    {
        result.clear();

        result.push_back(0);
        result.push_back(time);
        result.push_back(pos[0]);
        result.push_back(pos[1]);
        result.push_back(pos[2]);
        result.push_back(vel[0]);
        result.push_back(vel[1]);
        result.push_back(vel[2]);
        result.push_back(att[0]);
        result.push_back(att[1]);
        result.push_back(att[2]);
        navfile.dump(result);
    }

    {
        result.clear();

        result.push_back(time);
        result.push_back(bg[0]);
        result.push_back(bg[1]);
        result.push_back(bg[2]);
        result.push_back(ba[0]);
        result.push_back(ba[1]);
        result.push_back(ba[2]);
        result.push_back(state.sodo);
        errfile.dump(result);
    }
}

void imuInterpolation(const IMU &imu01, IMU &imu00, IMU &imu11, double mid) {
    double time = mid;

    double scale = (imu01.time - time) / imu01.dt;
    IMU buff     = imu01;

    imu00.time   = time;
    imu00.dt     = buff.dt - (buff.time - time);
    imu00.dtheta = buff.dtheta * (1 - scale);
    imu00.dvel   = buff.dvel * (1 - scale);
    imu00.odovel = buff.odovel * (1 - scale);

    imu11.time   = buff.time;
    imu11.dt     = buff.time - time;
    imu11.dtheta = buff.dtheta * scale;
    imu11.dvel   = buff.dvel * scale;
    imu11.odovel = buff.odovel * scale;
}

int isNeedInterpolation(const IMU &imu0, const IMU &imu1, double mid) {
    double time = mid;

    if (imu0.time < time && imu1.time > time) {
        double dt = time - imu0.time;
        // 前一个历元接�?
        // close to the first epoch
        if (dt < 0.0001) {
            return -1;
        }
        // 后一个历元接�?
        // close to the second epoch
        dt = imu1.time - time;
        if (dt < 0.0001) {
            return 1;
        }
        // 需内插
        // need interpolation
        return 2;
    }

    return 0;
}
